#include "stdafx.h"
#include "ColorSpaceRead.h"

// private function
WORD_SIZE colors2Bases(WORD_SIZE readInColors)
{
    WORD_SIZE readInBases = readInColors & 0x01; //Copy the first bit
    WORD_SIZE mask = 0x01;
    readInColors >>= 0x01; //shift the first encoded base bit
    for (int i = 0; i < CReadInBits::iReadLength; i++) {
        //get the ith digit and do the x
        WORD_SIZE nextBase = (readInBases ^ readInColors) & mask;
        mask <<= 0x01;
        readInBases += (nextBase << 0x01);
    }
    return(readInBases);
}

// Assum the first bits is encoded in readInColor
CReadInBits colors2Bases(CReadInBits readInColors)
{
    CReadInBits readInBases;
    readInBases.UpperBits = colors2Bases(readInColors.UpperBits);
    readInBases.LowerBits = colors2Bases(readInColors.LowerBits);

    return (readInBases);
}

CLongReadInBits colors2Bases(CLongReadInBits readInColors)
{
    CLongReadInBits readInBases;
    readInBases.first = colors2Bases(readInColors.first);
    // Set the first bit to continue decoding !!!
    CReadInBits setFlag = (readInBases.first >> (CReadInBits::iReadLength - 1));
    CReadInBits readInBaseColors2ndPart = (setFlag ^ readInColors.second);
    const unsigned int index = 0;
    CReadInBits readInColors2 = setIthBit(readInColors.second, setFlag, index);
    readInBases.second = colors2Bases(readInColors2);
    return (readInBases);
}

int getSNPtype(CReadInBits readInColors, CReadInBits refInColors)
{
    readInColors = readInColors.getPrefixStr((unsigned int)CReadInBits::iReadLength);
    refInColors = refInColors.getPrefixStr((unsigned int)CReadInBits::iReadLength); // compare only the first readlength bits
    WORD_SIZE upperBitsDiff = readInColors.UpperBits ^ refInColors.UpperBits;
    WORD_SIZE lowerBitsDiff = readInColors.LowerBits ^ refInColors.LowerBits;
    WORD_SIZE a = upperBitsDiff | lowerBitsDiff;

    int diff;
    // magic function to caculate how many ones are there
#ifdef __GNUC__
    // #ifdef AMD
    diff = __builtin_popcountll(a);
#else
    for (diff = 0; a; diff++) {
        a &= a - 1; // clear the least significant bit set
    }
#endif

    // check SNP type case
    if (diff >= 2 && diff <= 3) {
        // (1) Complement Type of SNP, (A <-> T, C<->G)
        // Indicated by two consecutive of color changed R <-> B or Y <-> G
        WORD_SIZE diffStr = upperBitsDiff & lowerBitsDiff;
        WORD_SIZE snpFlag = diffStr & (diffStr >> 1); // Note the first bit set is the position of SNP
        if (snpFlag) {
            return (-1);
        }

        // (2) Transversion Type of SNP, (A <-> C, G <-> T)
        // Indicated by two consecutive of color changed R <-> Y or B <-> G
        diffStr = ~upperBitsDiff & lowerBitsDiff;
        snpFlag = diffStr & (diffStr >> 1);
        if (snpFlag) {
            return (-2);
        }

        // (3) Transition Type of SNP, (A <-> G, C <-> T)
        // Indicated by two consecutive of color changed B <-> Y or R <-> G
        diffStr = upperBitsDiff & ~lowerBitsDiff;
        snpFlag = diffStr & (diffStr >> 1);
        if (snpFlag) {
            return (-3);
        }
    }
    return(diff);
}

bool encodeColors(const char* colorsStr, CReadInBits& readInColors)
{
    if (colorsStr[1] == '.') {
        return(false);
    }
    bool bNormalRead = true;
    const int startIndx = 1;
    if(encodePureColors(&colorsStr[1], readInColors, startIndx)) {
        char c = base2Color(colorsStr[0], colorsStr[1]);
        if (!setFirstBase(c, readInColors)) {
            bNormalRead = false;
        }
    } else {
        bNormalRead = false;
    }
    return(bNormalRead);
}

bool encodeColorsNas3(const char* colorsStr, CReadInBits& readInColors)
{
    bool bNormalRead = true;
    const int startIndx = 1;
    if(encodePureColorsNas3(&colorsStr[1], readInColors, startIndx)) {
        if(!setFirstBase(base2Color(colorsStr[0], colorsStr[1]), readInColors)) {
            bNormalRead = false;
        }
    } else {
        bNormalRead = false;
    }
    return(bNormalRead);
}

bool encodePureColors(const char* colorsStr, CReadInBits&  readInColors, int startIndex)
{
    readInColors.UpperBits = 0x00;
    readInColors.LowerBits = 0x00;
    const WORD_SIZE bit = 0x01;
    for (int i = startIndex; colorsStr[i] != '\0' ; i++) {
        switch (colorsStr[i]) {
        case '0':
            break;
        case '1':
            readInColors.LowerBits += (bit << i);
            break;
        case '2':
            readInColors.UpperBits += (bit << i);
            break;
        case '3':
            readInColors.UpperBits += (bit << i);
            readInColors.LowerBits += (bit << i);
            break;
        case 'N':
        case '.': // encode unknown as 3
            printf("Warning! Encode '.' in %s as color '3'\n", colorsStr);
            readInColors.UpperBits += (bit << i);
            readInColors.LowerBits += (bit << i);
            break;
        case '\0':
            break;
        default:
            return (false);
        }
    }
    return(true);
}

bool encodePureColorsNas3(const char* colorsStr, CReadInBits&  readInColors, int startIndex)
{
    readInColors.UpperBits = 0x00;
    readInColors.LowerBits = 0x00;
    const WORD_SIZE bit = 0x01;
    for (int i = startIndex; colorsStr[i] != '\0' ; i++) {
        switch (colorsStr[i]) {
        case '0':
            break;
        case '1':
            readInColors.LowerBits += (bit << i);
            break;
        case '2':
            readInColors.UpperBits += (bit << i);
            break;
        case '3':
            readInColors.UpperBits += (bit << i);
            readInColors.LowerBits += (bit << i);
            break;
        case 'N':
        case '.': // encode unknown as 3
            printf("Warning! Encode '.' in %s as color '3'\n", colorsStr);
            readInColors.UpperBits += (bit << i);
            readInColors.LowerBits += (bit << i);
            break;
        case '\0':
            break;
        default:
            return (false);
        }
    }
    return(true);
}

/*
 * This funciton translate a color string in CReadInBits to a string start with ACGT followed by 0123
 */
char* decodeColors(char* colorsStr, CReadInBits readInColors, int readLength)
{
    int i;
    for (i = 0; i < readLength; i++) {
        WORD_SIZE c = (readInColors.UpperBits & 0x01) << 1 | (readInColors.LowerBits & 0x01);
        if (i == 0) {
            switch (c) {
            case 0x00:
                colorsStr[0] = 'A';
                break;
            case 0x01:
                colorsStr[0] = 'C';
                break;
            case 0x02:
                colorsStr[0] = 'G';
                break;
            case 0x03:
                colorsStr[0] = 'T';
                break;
            default:
                colorsStr[0] = 'N';
            }
        } else {
            colorsStr[i] = '0' + (char)(c);
        }
        readInColors.LowerBits >>= 1;
        readInColors.UpperBits >>= 1;
    }
    colorsStr[i] = '\0';
    return(colorsStr);
}

/*
 * This funciton translate a color string to pure string of 0123
 */
char* decodePureColors(char* colorsStr, CReadInBits readInColors, int readLength)
{
    int i;
    for (i = 0; i < readLength; i++) {
        WORD_SIZE upperBit = (readInColors.UpperBits & 0x01);
        WORD_SIZE lowerBit = (readInColors.LowerBits & 0x01);
        WORD_SIZE c = upperBit * 2 + lowerBit;
        colorsStr[i] = '0' + (char)(c);
        readInColors.LowerBits >>= 1;
        readInColors.UpperBits >>= 1;
    }
    colorsStr[i] = '\0';
    return(colorsStr);
}

char* decodeColors(char* colorsStr, CLongReadInBits& r, int readLength)
{
    CReadInBits& readInColors1stHalf = r.first;
    CReadInBits& readInColors2ndHalf = r.second;
    int secondHalfStart = startOf2ndHalf((unsigned int)readLength);
    int halfReadLength = readLength - secondHalfStart;
    decodeColors(colorsStr, readInColors1stHalf, halfReadLength);
    decodePureColors(&colorsStr[secondHalfStart], readInColors2ndHalf, halfReadLength);
    return(colorsStr);
}

// In the following 4 funcitons r is not use, but for overloading bc future template
inline WORD_SIZE getLastBit(const CReadInBits& r)
{
    return(SHIFT_LEFT(0x01, (CReadInBits::iReadLength - 1)));
}

inline WORD_SIZE get2ndLastBit(const CReadInBits& r)
{
    return(SHIFT_LEFT(0x01, (CReadInBits::iReadLength - 1)) >> 0x01);
}

inline CReadInBits getLastBit(const CLongReadInBits& r)
{
    CReadInBits lastBit;
    // lastBit.UpperBits = 0x00
    lastBit.LowerBits = getLastBit(lastBit);
    return(lastBit);
}

inline CReadInBits get2ndLastBit(const CLongReadInBits& r)
{
    CReadInBits bit ;
    bit.LowerBits = getLastBit(bit) >> 0x01;
    return(bit);
}

// Non-consecutive set bit will be removed
inline WORD_SIZE removeSingleSetBit(WORD_SIZE diffStr)
{
    WORD_SIZE snpBits = diffStr & (diffStr >> 1);
    return(snpBits | (snpBits << 1));
}

inline CReadInBits removeSingleSetBit(CReadInBits diffStr)
{
    CReadInBits snpBits;
    snpBits.UpperBits = removeSingleSetBit(diffStr.UpperBits);
    snpBits.LowerBits = removeSingleSetBit(diffStr.LowerBits);
    // TODO: double check
    // Reset the middle bit is necessary
    WORD_SIZE flag = diffStr.LowerBits & (diffStr.UpperBits >> (CReadInBits::iReadLength - 1));
    snpBits.UpperBits = snpBits.UpperBits | (flag << (CReadInBits::iReadLength - 1));

    return(snpBits);
}

inline WORD_SIZE colorDiff2SnpType1(const CReadInBits& bitsDiff)
{
    // (1) Complement Type of SNP, (A <-> T, C<->G)
    WORD_SIZE diffStr = bitsDiff.UpperBits & bitsDiff.LowerBits;
    return(removeSingleSetBit(diffStr));
} // Indicated by two consecutive of color changed R <-> B or Y <-> G

inline WORD_SIZE colorDiff2SnpType2(const CReadInBits& bitsDiff)
{
    // (2) Transversion Type of SNP, (A <-> C, G <-> T)
    WORD_SIZE diffStr = ~(bitsDiff.UpperBits) & bitsDiff.LowerBits;
    return(removeSingleSetBit(diffStr));
} // Indicated by two consecutive of color changed R <-> Y or B <-> G

inline WORD_SIZE colorDiff2SnpType3(const CReadInBits& bitsDiff)
{
    // (3) Transition Type of SNP, (A <-> G, C <-> T)
    WORD_SIZE diffStr = bitsDiff.UpperBits & ~(bitsDiff.LowerBits);
    return(removeSingleSetBit(diffStr));
} // Indicated by two consecutive of color changed B <-> Y or R <-> G

inline CReadInBits colorDiff2SnpType1(const CLongReadInBits& bitsDiff)
{
    // (1) Complement Type of SNP, (A <-> T, C<->G)
    CReadInBits diffStr;
    diffStr.UpperBits = bitsDiff.first.UpperBits & bitsDiff.first.LowerBits;
    diffStr.LowerBits = bitsDiff.second.UpperBits & bitsDiff.second.LowerBits;
    return(removeSingleSetBit(diffStr));
} // Indicated by two consecutive of color changed R <-> B or Y <-> G

inline CReadInBits colorDiff2SnpType2(const CLongReadInBits& bitsDiff)
{
    // (2) Transversion Type of SNP, (A <-> C, G <-> T)
    CReadInBits diffStr;
    diffStr.UpperBits = ~(bitsDiff.first.UpperBits) & bitsDiff.first.LowerBits;
    diffStr.LowerBits = ~(bitsDiff.second.UpperBits) & bitsDiff.second.LowerBits;
    return(removeSingleSetBit(diffStr));
} // Indicated by two consecutive of color changed R <-> Y or B <-> G

inline CReadInBits colorDiff2SnpType3(const CLongReadInBits& bitsDiff)
{
    // (3) Transition Type of SNP, (A <-> G, C <-> T)
    CReadInBits diffStr;
    diffStr.UpperBits = bitsDiff.first.UpperBits & ~(bitsDiff.first.LowerBits);
    diffStr.LowerBits = bitsDiff.second.UpperBits & ~(bitsDiff.second.LowerBits);
    return(removeSingleSetBit(diffStr));
} // Indicated by two consecutive of color changed B <-> Y or R <-> G

inline int numOfBitSet(WORD_SIZE d)
{
    int diff;
#ifdef AMD
    diff = __builtin_popcountll(d); // magic function to count how many ones are there
#else
    for (diff = 0; d; diff++) {
        d &= d - 1; // clear the least significant bit set
    }
#endif
    return(diff);
}

inline int numOfBitSet(CReadInBits r)
{
    return(numOfBitSet(r.LowerBits) + numOfBitSet(r.UpperBits));
}

inline WORD_SIZE getConsecutiveSetBits(WORD_SIZE bits)
{
    return(bits & (bits >> 1));
}

inline CReadInBits getConsecutiveSetBits(CReadInBits bits)
{
    CReadInBits r;
    r.UpperBits = getConsecutiveSetBits(bits.UpperBits);
    r.LowerBits = getConsecutiveSetBits(bits.LowerBits);
    WORD_SIZE flag = bits.LowerBits & (bits.UpperBits >> (CReadInBits::iReadLength - 1));
    r.UpperBits = r.UpperBits | (flag << (CReadInBits::iReadLength - 1));
    // Set the last bit of UpperBits, which is the middle bit
    return(r);
}

inline bool hasConsecutiveSetBits(WORD_SIZE snpBits)
{
    return(getConsecutiveSetBits(snpBits) != 0);
}

inline bool hasConsecutiveSetBits(CReadInBits bits)
{
    CReadInBits b = getConsecutiveSetBits(bits);
    return(b.UpperBits != 0 || b.LowerBits != 0);
}

//TODO make template for the following 2 functions

// Correct the single color mismatch and adopted the valid SNP color
// return number of type of SNP it involved.
int correctReadInColorSpace(CReadInBits readInColors, CReadInBits refInColors, CReadInBits& correctedRead)
{
    // printBitsStr(readInColors, CReadInBits::iReadLength);// DEBUG
    // printBitsStr(refInColors, CReadInBits::iReadLength); // DEBUG
    // compare only the first read-length bits
    readInColors = readInColors.getPrefixStr((unsigned int)CReadInBits::iReadLength);
    refInColors  = refInColors.getPrefixStr((unsigned int)CReadInBits::iReadLength);
    correctedRead = refInColors; // Default is the same

    //bits string indicating which bits are different
    CReadInBits diffBits = readInColors ^ refInColors;
    WORD_SIZE d = diffBits.UpperBits | diffBits.LowerBits;
    int diff = numOfBitSet(d);
    WORD_SIZE lastBit = getLastBit(diffBits);
    WORD_SIZE last2ndBit = get2ndLastBit(diffBits);
    WORD_SIZE diffStr = diffBits.UpperBits | diffBits.LowerBits;
    WORD_SIZE snpBits = getConsecutiveSetBits(diffStr);
    if (isZero(0)) { // no consecutive mismatches
        if (!isZero(diffStr & lastBit)) { // TODO: TRIPLE CHECK, previously is ==, not &
            correctedRead = correctReadInColorSpaceSnpFlag(readInColors, refInColors, lastBit);
        }
        return(diff);
    } else if (hasConsecutiveSetBits(snpBits)) {
        correctedRead = refInColors;
        return(-1); // Three consecutive mismatches
    } else {
        WORD_SIZE replacedBits = 0x00;
        // Indicated by two consecutive of color changed R <-> B or Y <-> G
        replacedBits |= colorDiff2SnpType1(diffBits);
        // Indicated by two consecutive of color changed R <-> Y or B <-> G
        replacedBits |= colorDiff2SnpType2(diffBits);
        // Indicated by two consecutive of color changed B <-> Y or R <-> G
        replacedBits |= colorDiff2SnpType3(diffBits);
        /*
        {
            // (1) Complement Type of SNP, (A <-> T, C<->G)
            WORD_SIZE diffStr = diffBits.UpperBits & diffBits.LowerBits;;
            WORD_SIZE snpBits = diffStr & (diffStr >> 1);
            replacedBits |= ( snpBits | (snpBits << 1) );
        } // Indicated by two consecutive of color changed R <-> B or Y <-> G
        {
            // (2) Transversion Type of SNP, (A <-> C, G <-> T)
            WORD_SIZE diffStr = ~diffBits.UpperBits & diffBits.LowerBits;;
            WORD_SIZE snpBits = diffStr & (diffStr >> 1);
            replacedBits |= ( snpBits | (snpBits << 1) );
        } // Indicated by two consecutive of color changed R <-> Y or B <-> G
        {
            // (3) Transition Type of SNP, (A <-> G, C <-> T)
            WORD_SIZE diffStr = diffBits.UpperBits & ~diffBits.LowerBits;;
            WORD_SIZE snpBits = diffStr & (diffStr >> 1);
            replacedBits |= ( snpBits | (snpBits << 1) );
        } // Indicated by two consecutive of color changed B <-> Y or R <-> G
        */
        // if the bit before last bit is not mismatched
        bool lastBitDiff = (!isZero(diffStr & lastBit));
        if (lastBitDiff && isZero(diffStr & last2ndBit)) {
            // no consecutive mismatches at the end
            replacedBits |= lastBit; // take the last bit from read
        }
        correctedRead = correctReadInColorSpaceSnpFlag(readInColors, refInColors, replacedBits);

        // ASSERT
        CReadInBits read = colors2Bases(correctedRead);
        // printBitsStr(read, (unsigned int)CReadInBits::iReadLength);
        CReadInBits ref = colors2Bases(refInColors);
        // printBitsStr(ref, (unsigned int)CReadInBits::iReadLength);
        unsigned int NoSNP = bitsStrNCompare(read, ref, (unsigned int)CReadInBits::iReadLength);
        // assert(NoSNP <= 5);
        // assertSNP(type, refInColors, correctedRead);
        return(diff - (int)NoSNP * 2 + (int)lastBitDiff);
    }
}

// Correct the single color mismatch and adopted the valid SNP color
// return number of type of SNP it involved.
// TODO: see if it can be put into template with (CReadInBits & CLongReadInBits)
int correctReadInColorSpace(CLongReadInBits readInColors, CLongReadInBits refInColors, CLongReadInBits& correctedRead)
{
    // printBitsStr(readInColors, CReadInBits::iReadLength);// DEBUG
    // printBitsStr(refInColors, CReadInBits::iReadLength); // DEBUG
    // compare only the first read-length bits
    readInColors = readInColors.getPrefixStr((unsigned int)CReadInBits::iReadLength);
    refInColors  = refInColors.getPrefixStr((unsigned int)CReadInBits::iReadLength);
    correctedRead = refInColors; // Default is the same

    CLongReadInBits diffBits = readInColors ^ refInColors;
    CReadInBits diffStr;
    diffStr.UpperBits = diffBits.first.UpperBits | diffBits.first.LowerBits; //bits string indicating which bits are different
    diffStr.LowerBits = diffBits.second.UpperBits | diffBits.second.LowerBits; //bits string indicating which bits are different
    int diff = numOfBitSet(diffStr.UpperBits) + numOfBitSet(diffStr.LowerBits);

    CReadInBits lastBit = getLastBit(diffBits);
    CReadInBits last2ndBit = get2ndLastBit(diffBits);
    // WORD_SIZE diffStr = upperBitsDiff | lowerBitsDiff;
    CReadInBits snpBits = getConsecutiveSetBits(diffStr);
    if (isZero(snpBits)) { // no consecutive mismatches
        // TODO: triple comfirm, because previously is ==
        if (!isZero(diffStr & lastBit)) { // However, the last bit is difference, which indicates a base diff
            correctedRead = correctReadInColorSpaceSnpFlag(readInColors, refInColors, lastBit);
        }
        return(diff);
    } else if (hasConsecutiveSetBits(snpBits)) {
        correctedRead = refInColors;
        return(-1); // Three consecutive mismatches
    } else {
        CReadInBits replacedBits = 0x00;
        // Indicated by two consecutive of color changed R <-> B or Y <-> G
        replacedBits = (colorDiff2SnpType1(diffBits) | replacedBits);
        // Indicated by two consecutive of color changed R <-> Y or B <-> G
        replacedBits = (colorDiff2SnpType2(diffBits) | replacedBits);
        // Indicated by two consecutive of color changed B <-> Y or R <-> G
        replacedBits = (colorDiff2SnpType3(diffBits) | replacedBits);

        bool lastBitDiff = (!isZero(diffStr & lastBit));
        if (lastBitDiff && isZero(diffStr & last2ndBit) ) {
            // no consecutive mismatches at the end
            replacedBits = replacedBits | lastBit; // take the last bit from read
        }
        correctedRead = correctReadInColorSpaceSnpFlag(readInColors, refInColors, replacedBits);

        // ASSERT
        CLongReadInBits read = colors2Bases(correctedRead);
        // printBitsStr(read, (unsigned int)CReadInBits::iReadLength);
        CLongReadInBits ref = colors2Bases(refInColors);
        // printBitsStr(ref, (unsigned int)CReadInBits::iReadLength);
        unsigned int NoSNP = bitsStrNCompare(read, ref, (unsigned int)CReadInBits::iReadLength);
        // assert(NoSNP <= 5);
        // assertSNP(type, refInColors, correctedRead);
        return(diff - (int)NoSNP * 2 + (int)lastBitDiff);
    }
}

// Given strings in bases, return the corresponding color signal in A=0 C=1 G=2, T=3 representation
string readInBases2ColorsInACGT_Format(string readInBases)
{
    char colorsPresentInACGT[MAX_READ_LENGTH];
    CReadInBits r(readInBases.c_str());
    bases2Colors(r).decode(colorsPresentInACGT);
    return(string(colorsPresentInACGT));
}

// Given the color reads in ACGT format, return the corresponding read in 0123 format
string colorReadInACGTto0123Format(string colorReadInACGT)
{
    CReadInBits r(colorReadInACGT.c_str());
    char colorsIn0123Format[MAX_READ_LENGTH];
    // translate into 0123 format
    decodeColors(&(colorsIn0123Format[1]), r);
    colorsIn0123Format[0] = colorsIn0123Format[1];
    colorsIn0123Format[1] = '0';
    return(string(colorsIn0123Format));
}

// TEST
void assertSNP(int SNPType, CReadInBits refInColors, CReadInBits crInColors)
{
    if (SNPType == 0) {
        ASSERT_TRUE(refInColors == crInColors, "MISMATCHES after correction");
    } else {
        CReadInBits ref = colors2Bases(refInColors);
        CReadInBits cr = colors2Bases(crInColors);
        ref = ref.getPrefixStr(CReadInBits::iReadLength);
        cr = cr.getPrefixStr(CReadInBits::iReadLength);
        WORD_SIZE u = (cr.UpperBits ^ ref.UpperBits);
        WORD_SIZE l = (cr.LowerBits ^ ref.LowerBits);

        if (SNPType == 1) {
            ASSERT_TRUE((u & l) > 0,  "Not real complement SNP");
        } else if (SNPType == 2) {
            ASSERT_TRUE((~u & l) > 0, "Not real transverstion SNP");
        } else if (SNPType == 3) {
            ASSERT_TRUE((u & ~l) > 0, "Not real transition SNP");
        } else {
            ASSERT_TRUE(2 == bitsStrCompare(ref, cr), "Not double SNPs");
        }
    }
}

void colorQV2baseQV(CLongReadInBits readInColors, CLongReadInBits& correctedRead, char* Qscores, unsigned int readLength)
{
    if (Qscores[0] != '0') { // In case quality score are not available
        unsigned int halfOfReadLength = readLength - startOf2ndHalf(readLength);
        colorQV2baseQV(getDiffBits(readInColors.first, correctedRead.first), Qscores, halfOfReadLength);
        char* Qscores2 = &(Qscores[startOf2ndHalf(readLength)]);
        colorQV2baseQV(getDiffBits(readInColors.second, correctedRead.second), Qscores2, halfOfReadLength);
    }
}

void colorQV2baseQV(CReadInBits readInColors, CReadInBits& correctedRead, char* Qscores, unsigned int readLength)
{
    if (Qscores[0] != '0') { // In case quality score are not available
        colorQV2baseQV(getDiffBits(readInColors, correctedRead), Qscores, readLength); // TODO the read length is fixed under 64 now.
    }
}

// The input Q-scores, output is in the Phed char representation.
bool colorQV2baseQV(WORD_SIZE singleColorErrorflag, char* Qscores, unsigned int readLength)
{
    bool negativeQ = false;
    const char PhedScoreShift = 33;
    for (unsigned int i = 0; i < readLength; i++) {
        bool errorColorFlag = ((singleColorErrorflag & 0x01) > 0);
        Qscores[i] -= PhedScoreShift;
        if (errorColorFlag) {
            if (Qscores[i] > 0) {
                Qscores[i] *= (char)-1;
            } else if (Qscores[i] < 0) {
                Qscores[i] = 0;
                negativeQ = true;
            }
        }
        singleColorErrorflag >>= 0x01;
    }
    for (unsigned int i = 0; i < readLength; i++) {
        Qscores[i] = Qscores[i] + Qscores[i + 1];
        if (Qscores[i] < 0) {
            Qscores[i] = 0;
        }
        Qscores[i] += PhedScoreShift;
    }
    return(negativeQ);
}

void testLonglongShift(void)
{
    unsigned long long constNum = 1 << 30;
    unsigned long long word = constNum * 8;
    word = longlongShiftRight(word, 32);
    if ( word != 2) {
        cout << "testShift64Bit got unexpected result " << word << endl;
    } else {
        cout << "testShift64Bit got expected result " << word << endl;
    }
}

void testLongBases2ColorsCases(void)
{
    const char* read1 = "AAAACCCCGGGGTTTTAAAACCCCGGGGTTTTACGTACGTACGTACGTAAAACCCCGGGGTTTTA";
    const char* color1 ="A0001000300010003000100030001000313131313131313130001000300010003";
    testLongBases2Colors(read1, color1);
    const char* read2 = "AAAACCCCGGGGTTTTAAAACCCCGGGGTTTTTCGTACGTACGTACGTAAAACCCCGGGGTTTTAA";
    const char* color2 ="A00010003000100030001000300010000231313131313131300010003000100030";
    testLongBases2Colors(read2, color2);
}

void testLongBases2Colors(const char* longRead, const char* expLongColorSignals)
{
    unsigned int bufferLength = (unsigned int) strlen(longRead);
    unsigned int readLength = bufferLength - 1;
    if (isOdd(readLength)) {
        CReadInBits::iReadLength = (readLength + 1) / 2;
    } else {
        CReadInBits::iReadLength = readLength / 2;
    }
    CLongReadInBits rInBase;
    CLongReadInBits rInColors;
    // encodeLongRead(longRead, rInBase.first, rInBase.second);
    // TODO: complete this
    rInColors = bases2Colors(rInColors, readLength);

    char caBuf[FILENAME_MAX];
    decodeColors(caBuf, rInColors, readLength);
    if ( strcmp(expLongColorSignals, caBuf) != 0 ) {
        cout << "The decoded  read in color is:" << caBuf << endl;
        cout << "The expected read is color is:" << expLongColorSignals << endl;
    }
}

void testReverseColorSignals(const char* colorSignalStr)
{
    char caBuf[MAX_LINE];
    char caBuf2[MAX_LINE];
    strcpy(caBuf, colorSignalStr);
    CReadInBits::iReadLength = (int)strlen(caBuf) - 1;
    CReadInBits r;
    encodeColors(caBuf, r);
    decodeColors(caBuf2, r);
    cout << caBuf2 << endl;
    r = reverseColorRead(r);
    decodeColors(caBuf,r);
    cout << caBuf << endl;
}
